#include <ros/ros.h>

#include "cg_interfaces/ChassisSensor.h"
#include "cg_interfaces/ControlSignal.h"
#include "cg_interfaces/ChassisControl.h"

cg_interfaces::ChassisSensor sensor_msg;
cg_interfaces::ControlSignal signal_msg;

void OnChassisSensor(const cg_interfaces::ChassisSensor::ConstPtr &msg)
{
    sensor_msg = *msg;
}

void OnSignal(const cg_interfaces::ControlSignal::ConstPtr &msg)
{
    signal_msg = *msg;
}

int main(int argc, char **argv)
{
    const int depth = 5;
    ros::init(argc, argv, "dummy_core");

    ROS_INFO("control core : Dummy started");
    
    ros::NodeHandle nh;
    ros::Subscriber sensor_sub = nh.subscribe<cg_interfaces::ChassisSensor>("chassis_sensor", depth, OnChassisSensor);
    ros::Publisher control_pub = nh.advertise<cg_interfaces::ChassisControl>("chassis_control", depth);
    ros::Subscriber signal_sub = nh.subscribe<cg_interfaces::ControlSignal>("control_signal", depth, OnSignal);

    ros::Rate rate(50);
    while (ros::ok())
    {
        rate.sleep();
        int count = sensor_msg.bridge_count;
        // do something

        cg_interfaces::ChassisControl control_msg;
        control_msg.count = count;
        for (int i = 0; i < count; i++)
        {
            control_msg.bridges[i].id = i+1;
            control_msg.bridges[i].brake_force = 0.0;
            control_msg.bridges[i].steer_angle = 0.0;
            control_msg.bridges[i].torque_left = 0.0;
            control_msg.bridges[i].torque_right = 0.0;
        }
        control_pub.publish(control_msg);
        ros::spinOnce();
    }

    ros::shutdown();
    return 0;
}